# Example file showing a circle moving on screen
import pygame
from load_model import load_faces
import numpy as np
from face_lib import filter_unvisible_faces,zip_3d_to_2d
from key_rotation_in_pygame import get_3_axises_rotation
from rotation_transformation_around_word import rotation_degrees_to_rotation_matrix,rotate_by_matrix
from serial_port import SerialComunication
from queue import Queue,Empty
import threading


# pygame setup

pygame.init()
screen = pygame.display.set_mode((1280, 720))
clock = pygame.time.Clock()
running = True
dt = 0


faces = load_faces()

mouse_left_button_pressed = False

mouse_position_when_mouse_left_button_pressed=(0,0)
mouse_motion_speed = 1
mouse_motion_distance = 0

str_queue = Queue()
serialComunication = SerialComunication(str_queue)
serialComunication.connect("COM3")
serialComunication.start()


roll=0
pitch=0
yaw=0

#def receive_serial_data():
#    global roll,pitch,yaw
#    while True:
#        try:
#            roll,pitch,yaw = str_queue.get()    
#            #print(roll,pitch,yaw)
#        except Empty: 
#            print("未收到串口数据")    
#
#receive_thread = threading.Thread(target=receive_serial_data)
#receive_thread.start()
#
while running:
    # poll for events
    # pygame.QUIT event means the user clicked X to close your window
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False
#        if event.type == pygame.MOUSEBUTTONDOWN:
#            left,middle,right = pygame.mouse.get_pressed(num_buttons=3)
#            mouse_left_button_pressed = left
#            
#            if mouse_left_button_pressed:
#                mouse_position_when_mouse_left_button_pressed =  pygame.mouse.get_pos()
#            
#                
#                
#        if event.type == pygame.MOUSEMOTION:
#            if mouse_left_button_pressed:
#                mouse_relative_x,mouse_relative_y = pygame.mouse.get_pos()
#                mouse_motion_distance+=mouse_motion_speed*dt
#                rotate_x = rotate_around_x_by_degree(rotation_degree_x)
#                rotate_y = rotate_around_y_by_degree(rotation_degree_y)
#
#            
#

    # fill the screen with a color to wipe away anything from last frame
    screen.fill("white")

    # 获取三轴应该对应旋转的角度


    rotation_degree_x,rotation_degree_y,rotation_degree_z = serialComunication.roll_acc,serialComunication.pitch_acc,serialComunication.yaw
    print(rotation_degree_x,rotation_degree_y,rotation_degree_z)

    # 计算对应世界坐标系的旋转函数
    rotate_around_world_matrix = rotation_degrees_to_rotation_matrix(rotation_degree_x,rotation_degree_y,rotation_degree_z)

    # 0、把世界坐标系下的向量变换到相机坐标系下
    rx,ry,rz =get_3_axises_rotation(dt)
    rotate_around_camera_matrix = rotation_degrees_to_rotation_matrix(rx,ry,rz)

    # 把围绕世界坐标轴旋转的矩阵和围绕相机坐标轴旋转的矩阵合并为一个矩阵
    # 然后构建一个转换函数，用户对向量进行变换
    transform = rotate_by_matrix(np.dot(rotate_around_world_matrix,rotate_around_camera_matrix))

    # 1、对模型应用旋转操作
    rotated_faces = [([transform(vector) for vector in face ]) for face in faces]

    # 2、过滤模型不可见的面，留下可见的面
    visible_faces = filter_unvisible_faces(rotated_faces)

    # 3、把可见的面集合压缩为二维
    for coordinates,color in zip_3d_to_2d(visible_faces):

        pygame.draw.polygon(screen,
                            pygame.Color(*color) ,
                            [pygame.math.Vector2(coordinate) for coordinate in coordinates]
                            )



    # flip() the display to put your work on screen
    pygame.display.flip()

    # limits FPS to 60
    # dt is delta time in seconds since last frame, used for framerate-
    # independent physics.
    dt = clock.tick(60) / 1000

pygame.quit()